/*
 * Created on 01 decembre 2007
 * cedric.hartland(at)lri.fr
 *
 * This is demo for loading a map of a room with chairs and tables from an image file in Simbad. 
 * 
 */
package contribs.roomloader;

import java.io.Serializable;
import java.util.ArrayList;
import java.util.Random;

import javax.vecmath.Color3f;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import simbad.gui.Simbad;
import simbad.sim.Agent;
import simbad.sim.BallAgent;
import simbad.sim.EnvironmentDescription;
import simbad.sim.RangeSensorBelt;
import simbad.sim.RobotFactory;
import simbad.sim.Wall;
import contribs.maploader.SimpleImage;

/**
 * This class represents an Environment useable in Simbad but with the
 * specification of being defined by a PNG image. For the moment, the image is
 * defined as follows - 20x20 pixels (have to fit) - a red (rgb 255,0,0) pixel
 * means starting point for a robot - a green (rgb 0,255,0) pixel means a table -
 * a blue (rgb 0,0,255) pixel means a chair - black (rgb 0,0,0) pixel means a
 * wall - the image have to be in a PNG, JPG or GIF format.
 * 
 * @author cedric ( + nicolas )
 * 
 */
public class RoomLoaderDemo extends EnvironmentDescription implements Serializable {

	// static private Simbatch sim;
	// private Robot robot;

	// a goal to reach
	public Point3d _goal = new Point3d();
	// the initial starting points for robots (contains Point3d)
	public ArrayList _startingPoints = new ArrayList();

	public RoomLoaderDemo() {
		// build the environment
		// here a simple square-closed environment.
		Wall w1 = new Wall(new Vector3d(9, 0, 0), 19, 1, this);
		w1.rotate90(1);
		add(w1);
		Wall w2 = new Wall(new Vector3d(-9, 0, 0), 19, 2, this);
		w2.rotate90(1);
		add(w2);
		Wall w3 = new Wall(new Vector3d(0, 0, 9), 19, 1, this);
		add(w3);
		Wall w4 = new Wall(new Vector3d(0, 0, -9), 19, 2, this);
		add(w4);

		// create the robot

		add(new Robot(new Vector3d(0, 0, 0), "robot"));

	}

	public RoomLoaderDemo(String __filename, boolean optimized) {
		optimizedImageLoader(__filename);
	}

	/**
	 * initialize the environment due to the file __filename. TODO : - create
	 * multi-objectives points - create balls or non static items.
	 * 
	 * @param __filename
	 */
	public RoomLoaderDemo(String __filename) {
		int[] values;

		// load the image
		SimpleImage simpleImage = new SimpleImage(__filename, false);
		simpleImage.displayInformation();

		// step 2 : for each data in the image, initialize the environment
		// + starting position (for one or more robots)
		// + goal position
		for (int y = 0; y != simpleImage.getHeight(); y++) {
			System.out.print(" ");
			for (int x = 0; x != simpleImage.getWidth(); x++) {
				values = simpleImage.getPixel(x, y);
				if (values[1] > 200 && values[2] < 50 && values[3] < 50) {
					// red value, we will display a Wall in here :
					add(new Wall(new Vector3d(x - (simpleImage.getWidth() / 2), 0, y - (simpleImage.getHeight() / 2)), 1, 1, 1, this));
					System.out.print("#");
				} else if (values[1] < 50 && values[2] > 200 && values[3] < 50) {
					// green value : define the goal point
					_goal.x = x;
					_goal.z = y;
					_goal.y = 0;
					System.out.print("X");
				} else if (values[1] < 50 && values[2] < 50 && values[3] > 200) {
					_startingPoints.add(new Point3d(x, 0, y));
					// starting position
					System.out.print("!");
				} else if (values[1] < 100 && values[1] == values[2] && values[2] == values[3]) {
					// add a ball
					// take care because the setUsePhysics remove the
					// agentInspector
					showAxis(false);
					setUsePhysics(true);
					add(new BallAgent(new Vector3d(x - (simpleImage.getWidth() / 2), 0, y - (simpleImage.getHeight() / 2)), "ball", new Color3f(200, 0, 0), 0.25f, 0.25f));

				} else
					System.out.print(" ");
			}
			System.out.print("\n");
		}

		// add the robots of Robot instances
		for (int i = 0; i < _startingPoints.size(); i++) {
			add(new Robot(new Vector3d(((Point3d) _startingPoints.get(i)).x - (simpleImage.getWidth() / 2), 0f, ((Point3d) _startingPoints.get(i)).z - (simpleImage.getHeight() / 2)), "openDProbot"));
		}
	}

	/**
	 * This loader does not consider methodic exploration of the image but
	 * search for features (ie walls from (x,y) to (x2,y2)) and different
	 * objects of different sizes and orientations. It takes much more time to
	 * build but provide a lighter environment which is better for evolutionnary
	 * computations.
	 * 
	 * @param __filename
	 */
	public void optimizedImageLoader(String __filename) {
		// load the image
		SimpleImage simpleImage = new SimpleImage(__filename, false);
		simpleImage.displayInformation();

		searchWalls(simpleImage);
		searchTables(simpleImage);
		searchChairs(simpleImage);
		searchRobots(simpleImage);
	}

	/**
	 * a wall is defined by a black line
	 * 
	 * @param source
	 */
	private void searchWalls(SimpleImage source) {
		int[] values;
		int xx = 0, xxx = 0;
		int yy = 0, yyy = 0;
		wallColor = new Color3f(0.6f, 0.5f, 0.6f);
		for (int y = 0; y != source.getHeight(); y++) {
			for (int x = 0; x != source.getWidth(); x++) {
				values = source.getPixel(x, y);
				// dark/black value -> explore to find a wall
				if (values[1] < 10 & values[2] < 10 & values[3] < 10) {
					xx = x;
					yy = y;
					// compute k the higher value possible
					int k = 0;
					boolean limit = false;
					while (!limit) {
						k++;
						if ((x + k) < source.getWidth()) {
							int[] valuesTemp = source.getPixel(x + k, y);
							if (!(valuesTemp[1] == values[1] && valuesTemp[2] == values[2] && valuesTemp[3] == values[3]))
								limit = true;
							else {
								boolean neighbor = false;
								// check neighbors to remove the pixel or not
								if (y - 1 >= 0) {
									valuesTemp = source.getPixel(x + k, y - 1);
									if (valuesTemp[1] == values[1] && valuesTemp[2] == values[2] && valuesTemp[3] == values[3])
										neighbor = true;
								}
								if (y + 1 < source.getHeight()) {
									valuesTemp = source.getPixel(x + k, y + 1);
									if (valuesTemp[1] == values[1] && valuesTemp[2] == values[2] && valuesTemp[3] == values[3])
										neighbor = true;
								}
								if (!neighbor)
									source.setPixel(x + k, y, 255, 255, 255, 255);
							}
						} else
							limit = true;
					}
					// if
					// ((x+k)<source.getWidth()){source.setPixel(x+k,y,values[0],values[1],values[2],values[3]);}
					// if the wall is horizontal !!
					if (k > 1) {
						xxx = xx + k;
						add(new Wall(new Vector3d(x + (xxx - x) / 2d - (source.getWidth() / 2d), 0, y + 0.5 - (source.getHeight() / 2d)), Math.abs(xxx - xx) - 0.7f, 0.3f, 2, this));
						System.out.println("add a wall " + (xxx - xx));
					}
				}
			}
		}

		for (int y = 0; y != source.getHeight(); y++) {
			for (int x = 0; x != source.getWidth(); x++) {
				values = source.getPixel(x, y);
				// dark/black value -> explore to find a wall
				if (values[1] < 10 & values[2] < 10 & values[3] < 10) {
					xx = x;
					yy = y;
					// compute k the higher value possible
					// the wall might be vertical
					int k = 0;
					boolean limit = false;
					while (!limit) {
						k++;
						if ((y + k) < source.getHeight()) {
							int[] valuesTemp = source.getPixel(x, y + k);
							if (!(valuesTemp[1] == values[1] && valuesTemp[2] == values[2] && valuesTemp[3] == values[3]))
								limit = true;
							else
								source.setPixel(x, y + k, 255, 255, 255, 255);
						} else
							limit = true;
					}
					// if
					// ((y+k)<source.getHeight()){source.setPixel(x+k,y,values[3],values[0],values[1],values[2]);}

					// if the wall is horizontal !!
					if (k > 1) {
						yyy = yy + k;
						add(new Wall(new Vector3d(x + 0.5d - (source.getWidth() / 2d), 0, y + (yyy - y - source.getHeight()) / 2d), 0.3f, Math.abs(yyy - yy) - 0.7f, 2, this));
						System.out.println("add a wall " + (yyy - yy));
					}

				}
			}
		}
	}

	private void searchTables(SimpleImage source) {
		int[] values;
		int yy = 0;
		int xx = 0;
		for (int y = 0; y != source.getHeight(); y++) {
			for (int x = 0; x != source.getWidth(); x++) {
				values = source.getPixel(x, y);
				if (values[1] < 50 && values[2] > 200 && values[3] < 50) {
					yy = 0;
					xx = 0;
					// check for the size of the table
					boolean endline = false;
					while (!endline) {
						yy++;
						if ((y + yy) < source.getHeight()) {
							int[] valuesTemp = source.getPixel(x, y + yy);
							if (!(valuesTemp[0] == values[0] && valuesTemp[1] == values[1] && valuesTemp[2] == values[2]))
								endline = true;
						} else
							endline = true;
					}
					endline = false;
					while (!endline) {
						xx++;
						if ((x + xx) < source.getWidth()) {
							int[] valuesTemp = source.getPixel(x + xx, y);
							if (!(valuesTemp[0] == values[0] && valuesTemp[1] == values[1] && valuesTemp[2] == values[2]))
								endline = true;
						} else
							endline = true;
					}
					System.out.println("add a table at (" + x + "," + y + ") with " + xx + "," + yy);
					add(new Table(new Vector3d(x - (-xx + source.getWidth()) / 2d, 0, y - (-yy + source.getHeight()) / 2d), 0.22f, yy * 5, xx * 5, this));
					// clean the zone in the image :
					for (int i = x; i < x + xx; i++) {
						for (int j = y; j < y + yy; j++) {
							source.setPixel(i, j, 255, 255, 255, 255);
						}
					}

				}
			}
		}
	}

	private void searchChairs(SimpleImage source) {
		int[] values;
		Random r = new Random();
		for (int y = 0; y != source.getHeight(); y++) {
			for (int x = 0; x != source.getWidth(); x++) {
				values = source.getPixel(x, y);
				if (values[1] < 50 && values[2] < 50 && values[3] > 200) {
					Chair ch2 = new Chair(new Vector3d(x + 0.5 - source.getWidth() / 2d, 0, y + 0.5 - source.getWidth() / 2d), 0.22f, this);
					ch2.rotateY(r.nextDouble() * (2 * Math.PI));
					add(ch2);
				}
			}
		}
	}

	private void searchRobots(SimpleImage source) {
		int[] values;
		Random r = new Random();
		for (int y = 0; y != source.getHeight(); y++) {
			for (int x = 0; x != source.getWidth(); x++) {
				values = source.getPixel(x, y);
				if (values[1] > 200 && values[2] < 50 && values[3] < 50) {
					Robot r2d2 = new Robot(new Vector3d(x + 0.5 - source.getWidth() / 2d, 0, y + 0.5 - source.getWidth() / 2d), "testrobot");
					add(r2d2);

				}
			}
		}
	}

	public class Robot extends Agent {

		RangeSensorBelt sonars, bumpers;

		public Robot(Vector3d position, String name) {
			super(position, name);
			// Add sensors
			bumpers = RobotFactory.addBumperBeltSensor(this);
			sonars = RobotFactory.addSonarBeltSensor(this, 24);
		}

		/** Initialize Agent's Behavior */
		@Override
		public void initBehavior() {
			// nothing particular in this case
		}

		/** Perform one step of Agent's Behavior */
		@Override
		public void performBehavior() {

			if (bumpers.oneHasHit()) {
				setTranslationalVelocity(-0.1);
				setRotationalVelocity(0.1 * Math.random());

			} else {
				// Front left obstacle ?
				if (sonars.hasHit(0) && (sonars.hasHit(1))) {
					setRotationalVelocity(-Math.PI / 4);
					setTranslationalVelocity(0.1);
				}
				// Front right obstacle ?
				else if (sonars.hasHit(0) && (sonars.hasHit(23))) {
					setRotationalVelocity(Math.PI / 4);
					setTranslationalVelocity(0.1);
				}
				// left obstacle ?
				else if (sonars.hasHit(3) || sonars.hasHit(4)) {
					setRotationalVelocity(-Math.PI / 8);
					setTranslationalVelocity(0.1);
				}
				// right obstacle ?
				else if (sonars.hasHit(21) || sonars.hasHit(20)) {
					setRotationalVelocity(Math.PI / 8);
					setTranslationalVelocity(0.1);
				} else if ((getCounter() % 100) == 0) {
					setRotationalVelocity(Math.PI / 2 * (0.5 - Math.random()));
					setTranslationalVelocity(0.5);
				}
			}
		}
	}

	/*
	 * for test purposes
	 */
	public static void main(String[] args) {

		RoomLoaderDemo env = new RoomLoaderDemo("resources/room.png", true);
		// ImageLoaderEnvironment env = new
		// ImageLoaderEnvironment("resources/room3.png",true);
		new Simbad(env, false);
	}
}